from math import sin, cos, tan, pi, sqrt, atan
import matplotlib.pyplot as plt

from Task2_CAM_base_select import s_phi,v_phi,a_phi,a2r

plt.rc('font', family='Times New Roman')
plt.rc("axes", unicode_minus=False)
plt.rc("figure", figsize=(10,5))
plt.rc("savefig", dpi=400)
plt.rc("text", usetex = True)
ticklabels_style = {
    "fontname": "Times New Roman",
}

a = 0.1
d_phi = a/180.*pi
length = int(360/a)

# choose r0=20sqrt(2),e=30 s0=30
s0 = 25
e = 15
rt = 10

def x_phi(phi):
    return -(s0+s_phi(phi))*sin(phi) - e*cos(phi)
def y_phi(phi):
    return  (s0+s_phi(phi))*cos(phi) - e*sin(phi)

def x1(phi):
    return -e*cos(phi)
def y1(phi):
    return -e*sin(phi)

def x2(phi):
    return -sqrt(e**2+s0**2)*cos(phi)
def y2(phi):
    return -sqrt(e**2+s0**2)*sin(phi)

def x3(phi):
    return -sqrt((60+s0)**2+e**2)*cos(phi)
def y3(phi):
    return -sqrt((60+s0)**2+e**2)*sin(phi)

def dx_dphi(phi):
    return -(s0+s_phi(phi))*cos(phi) - (v_phi(phi)-e)*sin(phi)
def dy_dphi(phi):
    return -(s0+s_phi(phi))*sin(phi) + (v_phi(phi)-e)*cos(phi)

def d2x_dphi2(phi):
    return -(a_phi(phi)-s_phi(phi)-s0)*sin(phi) - (2*v_phi(phi)-e)*cos(phi)
def d2y_dphi2(phi):
    return  (a_phi(phi)-s_phi(phi)-s0)*cos(phi) - (2*v_phi(phi)-e)*sin(phi)

def rho_phi(phi):
    vdx_dphi = dx_dphi(phi)
    vdy_dphi = dy_dphi(phi)
    return ((vdx_dphi**2+vdy_dphi**2)**(3/2))/(vdx_dphi*d2y_dphi2(phi)-vdy_dphi*d2x_dphi2(phi))

def real_x_phi(phi):
    vdx_dphi = dx_dphi(phi)
    vdy_dphi = dy_dphi(phi)
    norm = sqrt(vdx_dphi**2+vdy_dphi**2)
    vdx_dphi/=norm
    vdy_dphi/=norm
    x = -vdy_dphi
    return x_phi(phi)+x*rt
def real_y_phi(phi):
    vdx_dphi = dx_dphi(phi)
    vdy_dphi = dy_dphi(phi)
    norm = sqrt(vdx_dphi**2+vdy_dphi**2)
    vdx_dphi/=norm
    vdy_dphi/=norm
    y = vdx_dphi
    return y_phi(phi)+y*rt


def alpha_phi(phi):
    return atan(abs(v_phi(phi)-e)/(s0+s_phi(phi)))*180/pi

r = list([rho_phi(d_phi*i) for i in range(length)])

gs_kw = dict(width_ratios=[1.5, 1], height_ratios=[1,1])

fig,axd = plt.subplot_mosaic(
    [[0,'alpha'],
     [0,'rho']], gridspec_kw=gs_kw
)

axd[0].set_aspect('equal')
axd['alpha'].set_ylim(0,70)
axd['alpha'].set_xlim(0,360)
axd['rho'].set_ylim(-200,200)
axd['rho'].set_xlim(0,360)
axd[0].plot(
    list([x_phi(d_phi*i) for i in range(length)]),
    list([y_phi(d_phi*i) for i in range(length)]), 'r-', lw=1, ms=0, label='theory edge')
axd[0].plot(
    list([real_x_phi(d_phi*i) for i in range(length)]),
    list([real_y_phi(d_phi*i) for i in range(length)]), 'k-', lw=1, ms=0, label='real edge')
axd[0].plot(
    list([x1(d_phi*i) for i in range(length)]),
    list([y1(d_phi*i) for i in range(length)]), 'y--', lw=1, ms=0, label='offset circle')
axd[0].plot(
    list([x2(d_phi*i) for i in range(length)]),
    list([y2(d_phi*i) for i in range(length)]), 'b--', lw=1, ms=0, label='base circle')
axd[0].plot(
    list([x3(d_phi*i) for i in range(length)]),
    list([y3(d_phi*i) for i in range(length)]), 'g--', lw=1, ms=0, label='far-dwelling circle')
axd['alpha'].plot(
    list([a*i for i in range(length)]),
    list([alpha_phi(d_phi*i) for i in range(length)]), 'r-', lw=1, ms=0, label='pressure angle')
axd['rho'].plot(
    list([a*i for i in range(263)]),
    list([rho_phi(d_phi*i) for i in range(263)]), 'r-', lw=1, ms=0, label='radius')
axd['rho'].plot(
    list([a*i for i in range(263,3121)]),
    list([rho_phi(d_phi*i) for i in range(263,3121)]), 'r-', lw=1, ms=0)
axd['rho'].plot(
    list([a*i for i in range(3121,3201)]),
    list([rho_phi(d_phi*i) for i in range(3121,3201)]), 'r-', lw=1, ms=0)
axd['rho'].plot(
    list([a*i for i in range(3201,length)]),
    list([rho_phi(d_phi*i) for i in range(3201,length)]), 'r-', lw=1, ms=0)
axd[0].set_xlabel('x/mm')
axd[0].set_ylabel('y/mm')
axd['alpha'].set_xlabel(r'$\varphi$/°')
axd['alpha'].set_ylabel('pressure angle/°')
axd['rho'].set_xlabel(r'$\varphi$/°')
axd['rho'].set_ylabel(r'$\rho$/mm')
for i in axd:
    axd[i].legend(loc = 'upper right')
plt.tight_layout()
plt.savefig('task2-CAM_draw.png')
plt.show()
pass